{
  "cells": [
    {
      "cell_type": "markdown",
      "id": "0238cc14",
      "metadata": {},
      "source": [
        "# Pose2 SLAM Example with Subgraph Solver (SPCG)\n"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "colab_button",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/Pose2SLAMwSPCG.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "colab_import",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n    import google.colab\n    %pip install --quiet gtsam-develop\nexcept ImportError:\n    pass"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "21f0dae1",
      "metadata": {},
      "source": [
        "This example demonstrates 2D Pose SLAM using the SubgraphSolver (SPCG) - a specialized \n",
        "iterative linear solver that can be more efficient for large sparse problems.\n",
        "\n",
        "**Key Features:**\n",
        "- Uses Levenberg-Marquardt as the outer optimization loop\n",
        "- Employs SubgraphSolver (SPCG) as the inner iterative linear solver\n",
        "- SPCG uses Preconditioned Conjugate Gradient with subgraph preconditioning\n",
        "- More memory efficient for large-scale SLAM problems\n",
        "\n",
        "The problem setup is identical to the standard Pose2 SLAM example:\n",
        "- 5 poses forming a square trajectory\n",
        "- Odometry measurements between consecutive poses  \n",
        "- A loop closure constraint"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "id": "89ae3db2",
      "metadata": {},
      "outputs": [],
      "source": [
        "import math\n",
        "import numpy as np\n",
        "import gtsam"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "7e9d3f5d",
      "metadata": {},
      "source": [
        "## Factor Graph Setup"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "id": "b853b91c",
      "metadata": {},
      "outputs": [],
      "source": [
        "# 1. Create a factor graph container and add factors to it\n",
        "graph = gtsam.NonlinearFactorGraph()\n",
        "\n",
        "# 2a. Add a prior on the first pose, setting it to the origin\n",
        "prior = gtsam.Pose2(0.0, 0.0, 0.0)  # prior at origin\n",
        "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))\n",
        "graph.add(gtsam.PriorFactorPose2(1, prior, prior_noise))"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "ccc891b3",
      "metadata": {},
      "source": [
        "## Odometry and Loop Closure Factors"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "id": "b213ddfd",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Factor Graph:\n",
            "NonlinearFactorGraph: size: 6\n",
            "\n",
            "Factor 0: PriorFactor on 1\n",
            "  prior mean:  (0, 0, 0)\n",
            "  noise model: diagonal sigmas [0.3; 0.3; 0.1];\n",
            "\n",
            "Factor 1: BetweenFactor(1,2)\n",
            "  measured:  (2, 0, 1.57079633)\n",
            "  noise model: diagonal sigmas [0.2; 0.2; 0.1];\n",
            "\n",
            "Factor 2: BetweenFactor(2,3)\n",
            "  measured:  (2, 0, 1.57079633)\n",
            "  noise model: diagonal sigmas [0.2; 0.2; 0.1];\n",
            "\n",
            "Factor 3: BetweenFactor(3,4)\n",
            "  measured:  (2, 0, 1.57079633)\n",
            "  noise model: diagonal sigmas [0.2; 0.2; 0.1];\n",
            "\n",
            "Factor 4: BetweenFactor(4,5)\n",
            "  measured:  (2, 0, 1.57079633)\n",
            "  noise model: diagonal sigmas [0.2; 0.2; 0.1];\n",
            "\n",
            "Factor 5: BetweenFactor(5,1)\n",
            "  measured:  (0, 0, 0)\n",
            "  noise model: diagonal sigmas [0.2; 0.2; 0.1];\n",
            "\n",
            "\n"
          ]
        }
      ],
      "source": [
        "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n",
        "graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2.0, 0.0, math.pi/2), odometry_noise))\n",
        "graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2.0, 0.0, math.pi/2), odometry_noise))\n",
        "graph.add(gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2.0, 0.0, math.pi/2), odometry_noise))\n",
        "graph.add(gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2.0, 0.0, math.pi/2), odometry_noise))\n",
        "\n",
        "# 2c. Add the loop closure constraint\n",
        "model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n",
        "graph.add(gtsam.BetweenFactorPose2(5, 1, gtsam.Pose2(0.0, 0.0, 0.0), model))\n",
        "\n",
        "print(\"\\nFactor Graph:\")\n",
        "print(graph)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "68432a1d",
      "metadata": {},
      "source": [
        "## Initial Estimate"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "id": "1632186c",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Initial Estimate:\n",
            "Values with 5 values:\n",
            "Value 1: (gtsam::Pose2)\n",
            "(0.5, 0, 0.2)\n",
            "\n",
            "Value 2: (gtsam::Pose2)\n",
            "(2.3, 0.1, 1.1)\n",
            "\n",
            "Value 3: (gtsam::Pose2)\n",
            "(2.1, 1.9, 2.8)\n",
            "\n",
            "Value 4: (gtsam::Pose2)\n",
            "(-0.3, 2.5, -2.08318531)\n",
            "\n",
            "Value 5: (gtsam::Pose2)\n",
            "(0.1, -0.7, -0.483185307)\n",
            "\n",
            "\n"
          ]
        }
      ],
      "source": [
        "initial_estimate = gtsam.Values()\n",
        "initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))\n",
        "initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, 1.1))\n",
        "initial_estimate.insert(3, gtsam.Pose2(2.1, 1.9, 2.8))\n",
        "initial_estimate.insert(4, gtsam.Pose2(-0.3, 2.5, 4.2))\n",
        "initial_estimate.insert(5, gtsam.Pose2(0.1, -0.7, 5.8))\n",
        "\n",
        "print(\"\\nInitial Estimate:\")\n",
        "print(initial_estimate)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "865fd340",
      "metadata": {},
      "source": [
        "## Optimization with SubgraphSolver (SPCG)\n",
        "\n",
        "The key difference from standard optimization is the use of SubgraphSolver as the  iterative linear solver. This can be more efficient for large sparse problems."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 14,
      "id": "3989cf2b",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Initial error: 130.17500174\n",
            "newError: 5.78644755128\n",
            "errorThreshold: 5.78644755128 > 0\n",
            "absoluteDecrease: 124.388554189 >= 1e-05\n",
            "relativeDecrease: 0.955548703868 >= 1e-05\n",
            "newError: 0.00526196818429\n",
            "errorThreshold: 0.00526196818429 > 0\n",
            "absoluteDecrease: 5.78118558309 >= 1e-05\n",
            "relativeDecrease: 0.999090639267 >= 1e-05\n",
            "newError: 1.53140303232e-08\n",
            "errorThreshold: 1.53140303232e-08 > 0\n",
            "absoluteDecrease: 0.00526195287026 >= 1e-05\n",
            "relativeDecrease: 0.999997089676 >= 1e-05\n",
            "newError: 2.78539546447e-21\n",
            "errorThreshold: 2.78539546447e-21 > 0\n",
            "absoluteDecrease: 1.53140303232e-08 < 1e-05\n",
            "relativeDecrease: 1 >= 1e-05\n",
            "converged\n",
            "errorThreshold: 2.78539546447e-21 <? 0\n",
            "absoluteDecrease: 1.53140303232e-08 <? 1e-05\n",
            "relativeDecrease: 1 <? 1e-05\n",
            "iterations: 4 >? 100\n"
          ]
        }
      ],
      "source": [
        "parameters = gtsam.LevenbergMarquardtParams()\n",
        "parameters.setVerbosity(\"ERROR\")\n",
        "parameters.setVerbosityLM(\"LAMBDA\")\n",
        "\n",
        "# Specify \"ITERATIVE\" to use an iterative linear solver.\n",
        "parameters.setLinearSolverType(\"ITERATIVE\")\n",
        "parameters.setIterativeParams(gtsam.SubgraphSolverParameters())\n",
        "\n",
        "optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, parameters)\n",
        "result = optimizer.optimize()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 15,
      "id": "d4dcf5e9",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Final Result:\n",
            "Values with 5 values:\n",
            "Value 1: (gtsam::Pose2)\n",
            "(3.23784789028e-15, 9.85867113204e-15, -8.54048365546e-16)\n",
            "\n",
            "Value 2: (gtsam::Pose2)\n",
            "(2, 1.50087829433e-12, 1.57079632679)\n",
            "\n",
            "Value 3: (gtsam::Pose2)\n",
            "(2, 2, 3.14159265359)\n",
            "\n",
            "Value 4: (gtsam::Pose2)\n",
            "(9.467927256e-12, 2, -1.57079632679)\n",
            "\n",
            "Value 5: (gtsam::Pose2)\n",
            "(3.80509879214e-15, 1.06998239513e-14, -8.33727794179e-16)\n",
            "\n",
            "\n",
            "subgraph solver final error = 2.7853954644738e-21\n"
          ]
        }
      ],
      "source": [
        "print(\"\\nFinal Result:\")\n",
        "print(result)\n",
        "print(f\"subgraph solver final error = {graph.error(result)}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": ".venv",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.9"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}